;;==========================================================================
;; Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
;;==========================================================================

#include <bsp/bspcfg.h>
#include <kcfg.h>

//
// PSR
//
#define __32BIT(n)      (1 << (n))
#define PSR_FLAG_MASK   0xf0000000
#define PSR_FLAG_N      __32BIT(31)
#define PSR_FLAG_Z      __32BIT(30)
#define PSR_FLAG_C      __32BIT(29)
#define PSR_FLAG_V      __32BIT(28)

#define PSR_I           __32BIT(7)
#define PSR_F           __32BIT(6)

#define PSR_T           __32BIT(5)

#define PSR_MODE_MASK   0x0000001f
#define PSR_MODE_USR    0x00000010
#define PSR_MODE_FIQ    0x00000011
#define PSR_MODE_IRQ    0x00000012
#define PSR_MODE_SVC    0x00000013
#define PSR_MODE_ABT    0x00000017
#define PSR_MODE_UND    0x0000001b
#define PSR_MODE_SYS    0x0000001f



    AREA |.text|,ALIGN=2,CODE, READONLY

#define DECL_ASMENTRY(label)        label

#define SET_EXCEPTION_HANDLER(handler) \
    ldr     pc, AddressOf##handler

#define DEFINE_EXCEPTION_ADDRESS(handler) \
AddressOf##handler DCD handler

    EXPORT  VectorTableBegin
    EXPORT  VectorTableEnd

DECL_ASMENTRY(VectorTableBegin)

    SET_EXCEPTION_HANDLER(RSTExceptionHandler)
    SET_EXCEPTION_HANDLER(UNDExceptionHandler)
    SET_EXCEPTION_HANDLER(SWIExceptionHandler)
    SET_EXCEPTION_HANDLER(PABTExceptionHandler)
    SET_EXCEPTION_HANDLER(DABTExceptionHandler)
    SET_EXCEPTION_HANDLER(ADRExceptionHandler)
    SET_EXCEPTION_HANDLER(IRQExceptionHandler)
    SET_EXCEPTION_HANDLER(FIQExceptionHandler)

DEFINE_EXCEPTION_ADDRESS(RSTExceptionHandler)
DEFINE_EXCEPTION_ADDRESS(UNDExceptionHandler)
DEFINE_EXCEPTION_ADDRESS(SWIExceptionHandler)
DEFINE_EXCEPTION_ADDRESS(PABTExceptionHandler)
DEFINE_EXCEPTION_ADDRESS(DABTExceptionHandler)
DEFINE_EXCEPTION_ADDRESS(ADRExceptionHandler)
DEFINE_EXCEPTION_ADDRESS(IRQExceptionHandler)
DEFINE_EXCEPTION_ADDRESS(FIQExceptionHandler)

DECL_ASMENTRY(VectorTableEnd)

    IMPORT  g_undStack
    IMPORT  g_abtStack
    IMPORT  g_irqStack

undStack    DCD g_undStack
abtStack    DCD g_abtStack
irqStack    DCD g_irqStack

    IMPORT  DoRSTException
RSTExceptionHandler

    b       DoRSTException

DECL_ASMENTRY(UNDExceptionHandler)

    ldr     sp, undStack
    str     lr, [sp]            // save lr
    mrs     lr, spsr
    str     lr, [sp, #4]        // save spsr

    mov     sp, #(PSR_I + PSR_F + PSR_MODE_SVC)
    msr     spsr, sp            // switch to svc mode

    and     lr, lr, #0xf
    ldr     lr, [pc, lr, lsl #2]
    movs    pc, lr              // change mode and branch

UNDDispatchTable
    DCD UNDFromUSR              // 0
    DCD UNDFromINV              // 1
    DCD UNDFromINV              // 2
    DCD UNDFromSVC              // 3
    DCD UNDFromINV              // 4
    DCD UNDFromINV              // 5
    DCD UNDFromINV              // 6
    DCD UNDFromINV              // 7
    DCD UNDFromINV              // 8
    DCD UNDFromINV              // 9
    DCD UNDFromINV              // 10
    DCD UNDFromINV              // 11
    DCD UNDFromINV              // 12
    DCD UNDFromINV              // 13
    DCD UNDFromINV              // 14
    DCD UNDFromINV              // 15

DECL_ASMENTRY(SWIExceptionHandler)

    and     r5, r5, #0x3
    ldr     r5, [pc, r5, lsl #2]
    mov     pc, r5

FastSystemCallDispatchTable
    DCD NormalSystemCall        // 0
    DCD FastSystemCall_SwpAdd   // 1
    DCD FastSystemCall_CmpSwp   // 2
    DCD NormalSystemCall        // 3

DECL_ASMENTRY(FastSystemCall_SwpAdd)

    cmp     r0, #KCONFIG_KERNEL_BASE
    ldrlo   r2, [r0]
    addlo   r2, r2, r1
    swplo   r2, r2, [r0]
    movlo   r0, r2
    movs    pc, lr

DECL_ASMENTRY(FastSystemCall_CmpSwp)

    cmp     r0, #KCONFIG_KERNEL_BASE
    bhs     FastSystemCall_CmpSwp_Exit
    ldr     r3, [r0]
    cmp     r3, r2
    streq   r1, [r0]
    mov     r0, r3
FastSystemCall_CmpSwp_Exit
    movs    pc, lr

DECL_ASMENTRY(NormalSystemCall)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    add     r8, sp, #60         // offsetof(InterruptContext, pc) = #60
    stmdb   r8, {sp, lr}^       // save sp, lr
    str     lr, [sp, #60]       // save pc
    mrs     r8, spsr
    str     r8, [sp, #64]       // save cpsr
                                // offsetof(InterruptContext, cpsr) = #64
    mov     fp, #0

    bic     lr, lr, #0
    ldr     r7, [lr, #-4]
    and     r7, r7, #0x00ffffff // get system call no

    mov     r8, #PSR_MODE_SVC
    msr     cpsr_c, r8          // enable irq

    cmp     r7, #24             // c_uNumOfSystemCall = #24
    ldrcs   r0, InvalidArgument // set r0 = E_INVALID_ARGUMENT
    bcs     ReturnFromSystemCall


    str     r4, [sp, #-4]!      // push arg5
    adr     lr, SWIExceptionHandler_Next
    ldr     r5, SystemCallTable
    ldr     pc, [r5, r7, lsl #2]
SWIExceptionHandler_Next
    add     sp, sp, #4
    str     r0, [sp, #0]        // offsetof(InterruptContext, r0) = #0


    b       ReturnFromSystemCall

    IMPORT  g_fnSystemCalls

SystemCallTable DCD g_fnSystemCalls
InvalidArgument DCD 0x80070057


DECL_ASMENTRY(PABTExceptionHandler)

    ldr     sp, abtStack
    sub     lr, lr, #4
    str     lr, [sp]            // save lr
    mrs     lr, spsr
    str     lr, [sp, #4]        // save spsr

    mov     sp, #(PSR_I + PSR_F + PSR_MODE_SVC)
    msr     spsr, sp            // switch to svc mode

    and     lr, lr, #0xf
    ldr     lr, [pc, lr, lsl #2]
    movs    pc, lr              // change mode and branch

PABTDispatchTable
    DCD PABTFromUSR             // 0
    DCD PABTFromINV             // 1
    DCD PABTFromINV             // 2
    DCD PABTFromSVC             // 3
    DCD PABTFromINV             // 4
    DCD PABTFromINV             // 5
    DCD PABTFromINV             // 6
    DCD PABTFromINV             // 7
    DCD PABTFromINV             // 8
    DCD PABTFromINV             // 9
    DCD PABTFromINV             // 10
    DCD PABTFromINV             // 11
    DCD PABTFromINV             // 12
    DCD PABTFromINV             // 13
    DCD PABTFromINV             // 14
    DCD PABTFromINV             // 15

DECL_ASMENTRY(DABTExceptionHandler)

    ldr     sp, abtStack
    sub     lr, lr, #8
    str     lr, [sp]            // save lr
    mrs     lr, spsr
    str     lr, [sp, #4]        // save spsr

    mov     sp, #(PSR_I + PSR_F + PSR_MODE_SVC)
    msr     spsr, sp            // switch to svc mode

    and     lr, lr, #0xf
    ldr     lr, [pc, lr, lsl #2]
    movs    pc, lr              // change mode and branch

DABTDispatchTable
    DCD DABTFromUSR             // 0
    DCD DABTFromINV             // 1
    DCD DABTFromINV             // 2
    DCD DABTFromSVC             // 3
    DCD DABTFromINV             // 4
    DCD DABTFromINV             // 5
    DCD DABTFromINV             // 6
    DCD DABTFromINV             // 7
    DCD DABTFromINV             // 8
    DCD DABTFromINV             // 9
    DCD DABTFromINV             // 10
    DCD DABTFromINV             // 11
    DCD DABTFromINV             // 12
    DCD DABTFromINV             // 13
    DCD DABTFromINV             // 14
    DCD DABTFromINV             // 15

    IMPORT  DoADRException
DECL_ASMENTRY(ADRExceptionHandler)

    b       DoADRException

DECL_ASMENTRY(IRQExceptionHandler)

    ldr     sp, irqStack
    sub     lr, lr, #4
    str     lr, [sp]            // save lr
    mrs     lr, spsr
    str     lr, [sp, #4]        // save spsr

    mov     sp, #(PSR_I + PSR_F + PSR_MODE_SVC)
    msr     spsr, sp            // switch to svc mode

    and     lr, lr, #0xf
    ldr     lr, [pc, lr, lsl #2]
    movs    pc, lr              // change mode and branch

IRQDispatchTable
    DCD IRQFromUSR              // 0
    DCD IRQFromINV              // 1
    DCD IRQFromINV              // 2
    DCD IRQFromSVC              // 3
    DCD IRQFromINV              // 4
    DCD IRQFromINV              // 5
    DCD IRQFromINV              // 6
    DCD IRQFromINV              // 7
    DCD IRQFromINV              // 8
    DCD IRQFromINV              // 9
    DCD IRQFromINV              // 10
    DCD IRQFromINV              // 11
    DCD IRQFromINV              // 12
    DCD IRQFromINV              // 13
    DCD IRQFromINV              // 14
    DCD IRQFromINV              // 15

DECL_ASMENTRY(FIQExceptionHandler)

    ldr     sp, irqStack
    sub     lr, lr, #4
    str     lr, [sp]            // save lr
    mrs     lr, spsr
    str     lr, [sp, #4]        // save spsr

    mov     sp, #(PSR_I + PSR_F + PSR_MODE_SVC)
    msr     spsr, sp            // switch to svc mode

    and     lr, lr, #0xf
    ldr     lr, [pc, lr, lsl #2]
    movs    pc, lr              // change mode and branch

FIQDispatchTable
    DCD IRQFromUSR             // 0
    DCD IRQFromINV             // 1
    DCD IRQFromSVC             // 2
    DCD IRQFromSVC             // 3
    DCD IRQFromINV             // 4
    DCD IRQFromINV             // 5
    DCD IRQFromINV             // 6
    DCD IRQFromINV             // 7
    DCD IRQFromINV             // 8
    DCD IRQFromINV             // 9
    DCD IRQFromINV             // 10
    DCD IRQFromINV             // 11
    DCD IRQFromINV             // 12
    DCD IRQFromINV             // 13
    DCD IRQFromINV             // 14
    DCD IRQFromINV             // 15

DECL_ASMENTRY(PABTFromINV)
DECL_ASMENTRY(DABTFromINV)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - lr}       // save r0 - lr
    ldr     r4, abtStack
    b       CallReportInvalidMode


DECL_ASMENTRY(IRQFromINV)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - lr}       // save r0 - lr
    ldr     r4, irqStack
    b       CallReportInvalidMode

DECL_ASMENTRY(UNDFromINV)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - lr}       // save r0 - lr
    ldr     r4, undStack


    IMPORT  ReportInvalidMode

DECL_ASMENTRY(CallReportInvalidMode)

    mov     fp, #0
    ldmia   r4, {r5 - r6}       // get pc, cpsr
    add     r4, sp, #60         // offsetof(InterruptContext, pc) = #60
    stmia   r4, {r5 - r6}       // save pc, cpsr

    mov     r0, sp
    bl      ReportInvalidMode


    IMPORT  HandleException

DECL_ASMENTRY(UNDFromUSR)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    ldr     r4, undStack
    add     r8, sp, #60         // offsetof(InterruptContext, pc) = #60
    ldmia   r4, {r5 - r6}       // get saved pc, cpsr
    stmia   r8, {r5 - r6}       // save pc, cpsr
    stmdb   r8, {sp, lr}^       // save sp_usr, lr_usr
    mov     fp, #0

    mov     r0, #(PSR_MODE_SVC)
    msr     cpsr_c, r0          // enable irq

    mov     r0, #0              // ExceptionVector_UndefinedInstruction = #0
    mov     r1, #0
    mov     r2, sp
    adr     lr, ReturnFromUSRException
    b       HandleException


DECL_ASMENTRY(PABTFromUSR)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    ldr     r4, abtStack
    add     r8, sp, #60         // offsetof(InterruptContext, pc) = #60
    ldmia   r4, {r5 - r6}       // get saved pc, cpsr
    stmia   r8, {r5 - r6}       // save pc, cpsr
    stmdb   r8, {sp, lr}^       // save sp_usr, lr_usr
    mov     fp, #0

    mov     r0, #(PSR_MODE_SVC)
    msr     cpsr_c, r0          // enable irq

    mov     r0, #1              // ExceptionVector_PrefetchAbort = #1
    mov     r1, #0
    mov     r2, sp
    adr     lr, ReturnFromUSRException
    b       HandleException


DECL_ASMENTRY(DataAbort)
// In:  r0 = address of aborted instruction
// Out: r1 != 0 if writing

    ldr     r4, [r0]            // Read the aborted instruction
    tst     r4, r4, lsr #21     // C = bit 20
    sbc     r1, r1, r1          // r1 = C - 1

#if defined(_arm720)

    and     r2, r4, #0xf << 24
    add     pc, pc, r2, lsr #22
    nop

    b       DataAbort_LdrStrSH_Post         // 0b0000
    b       DataAbort_LdrStrSH_Pre          // 0b0001
    b       DataAbort_Invalid               // 0b0010
    b       DataAbort_Invalid               // 0b0011
    b       DataAbort_LdrStr_Offset_Post    // 0b0100
    b       DataAbort_LdrStr_Offset_Pre     // 0b0101
    b       DataAbort_LdrStr_Rm_Post        // 0b0110
    b       DataAbort_LdrStr_Rm_Pre         // 0b0111
    b       DataAbort_LdmStm                // 0b1000
    b       DataAbort_LdmStm                // 0b1001
    b       DataAbort_Invalid               // 0b1010
    b       DataAbort_Invalid               // 0b1011
    b       DataAbort_Normal                // 0b1100
    b       DataAbort_Normal                // 0b1101
    b       DataAbort_Invalid               // 0b1111

DECL_ASMENTRY(DataAbort_LdrStrSH_Pre)

    tst     r4, #1 << 21                    // Check the W bit
    beq     DataAbort_Normal

DECL_ASMENTRY(DataAbort_LdrStrSH_Post)

    and     r5, r4, #0x00f          // Get Imm[3:0] / Rm
    tst     r4, #1 << 22            // Check the # bit
    andne   r2, r4, #0xf00          // Get Imm[7:4] and combine Imm[3:0]
    orrne   r2, r5, r2, lsr #4
    ldreq   r2, [sp, r5, lsl #2]    // Or load Rm value
    and     r5, r4, #0xf << 16      // Get Rn
    ldr     r0, [sp, r5, lsr #14]   // load Rn value
    tst     r4, #1 << 23            // Check the U bit
    subne   r7, r0, r2
    addeq   r7, r0, r2
    b       DataAbort_SaveR7

DECL_ASMENTRY(DataAbort_LdrStr_Offset_Pre)

    tst     r4, #1 << 21            // Check the W bit
    beq     DataAbort_Normal

DECL_ASMENTRY(DataAbort_LdrStr_Offset_Post)

    movs    r2, r4, lsl #20         // Get offset
    beq     DataAbort_Normal
    and     r5, r4, #0xf << 16      // Get Rn
    ldr     r0, [sp, r5, lsr #14]
    tst     r4, #1 << 23            // Check the U bit
    subne   r7, r0, r2, lsr #20
    addeq   r7, r0, r2, lsr #20
    b       DataAbort_SaveR7

DECL_ASMENTRY(DataAbort_LdrStr_Rm_Pre)

    tst     r4, #1 << 21            // Check the W bit
    beq     DataAbort_Normal

DECL_ASMENTRY(DataAbort_LdrStr_Rm_Post)

    and     r5, r4, #0xf
    ldr     r2, [sp, r5, lsl #2]    // Get Rm
    mov     r3, r4, lsr #7
    ands    r3, r3, #0x1f           // Get ShiftImm
    and     r6, r4, #0x70
    orreq   r6, r6, #8
    add     pc, pc, r6
    nop

    mov     r2, r2, lsl r3              // 0: LSL(0) && ShiftImm != 0
    b       DataAbort_LdrStr_Rm_Next

    b       DataAbort_LdrStr_Rm_Next   // 1: LSL(0) && ShiftImm == 0
    nop

    b       DataAbort_LdrStr_Rm_Next    // 2: ???
    nop

    b       DataAbort_LdrStr_Rm_Next   // 3: ???
    nop

    mov     r2, r2, lsr r3              // 4: LSR(1) && ShiftImm != 0
    b       DataAbort_LdrStr_Rm_Next

    mov     r2, r2, lsr #32             // 5: LSR(1) && ShiftImm == 32
    b       DataAbort_LdrStr_Rm_Next

    b       DataAbort_LdrStr_Rm_Next   // 6: ???
    nop

    b       DataAbort_LdrStr_Rm_Next   // 7: ???
    nop

    mov     r2, r2, asr r3              // 8: ASR(2) && ShiftImm != 0
    b       DataAbort_LdrStr_Rm_Next

    mov     r2, r2, asr #32             // 9: ASR(2) && ShiftImm == 32
    b       DataAbort_LdrStr_Rm_Next

    b       DataAbort_LdrStr_Rm_Next   // 10: ???
    nop

    b       DataAbort_LdrStr_Rm_Next   // 11: ???
    nop

    mov     r2, r2, ror r3              // 12: ROR(3) && ShiftImm != 0
    b       DataAbort_LdrStr_Rm_Next

    mov     r2, r2, rrx                 // 13: RRX(3 && ShiftImm == 0)
    b       DataAbort_LdrStr_Rm_Next

    nop                                 // 14: ???
    nop

    nop                                 // 15: ???

DataAbort_LdrStr_Rm_Next
    and     r5, r4, #0xf << 16          // Get Rn
    ldr     r0, [sp, r5, lsr #14]
    tst     r4, #1 << 23                // Check the U bit
    subne   r7, r0, r2
    addeq   r7, r0, r2
    b       DataAbort_SaveR7

DECL_ASMENTRY(DataAbort_LdmStm)

    tst     r4, #1 << 21                // Check the W bit
    beq     DataAbort_Normal

    mov     r7, #0x11
    orr     r7, r7, r7, lsl #8
    and     r0, r4, r7                  // Get register list
    and     r2, r4, r7, lsl #1
    add     r0, r0, r2, lsr #1
    and     r2, r4, r7, lsl #2
    add     r0, r0, r2, lsr #2
    and     r2, r4, r7, lsl #3
    add     r0, r0, r2, lsr #3
    add     r0, r0, r0, lsr #8
    add     r0, r0, r0, lsr #4
    and     r7, r0, #0xf                // r7 = No of registers to transfer
    and     r5, r4, #0xf << 16          // Get Rn
    ldr     r0, [sp, r5, lsr #14]
    tst     r4, #1 << 23                // Check the U bit
    subne   r7, r0, r7, lsl #2
    addeq   r7, r0, r7, lsl #2

DECL_ASMENTRY(DataAbort_SaveR7)

    str     r7, [sp, r5, lsr #14]       // Put R7

DECL_ASMENTRY(DataAbort_Normal)

    mov     pc, lr

    IMPORT  ReportInvalidInstruction
DECL_ASMENTRY(DataAbort_Invalid)

    bl      ReportInvalidInstruction

    mov     pc, lr


#elif defined(_arm920) || defined(_xscale)

    mov     pc, lr


#else
#error Unknown architecture variant
#endif

DECL_ASMENTRY(DABTFromUSR)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    ldr     r4, abtStack
    add     r8, sp, #60         // offsetof(InterruptContext) = #60
    ldmia   r4, {r5 - r6}       // get saved pc, cpsr
    stmia   r8, {r5 - r6}       // save pc, cpsr
    stmdb   r8, {sp, lr}^       // save sp_usr, lr_usr
    mov     fp, #0

    mov     r0, #(PSR_MODE_SVC)
    msr     cpsr_c, r0          // enable irq

    mov     r0, r5
    bl      DataAbort

    mov     r0, #2              // ExceptionVectorDataAbort = #2
    mov     r2, sp
    adr     lr, ReturnFromUSRException
    b       HandleException


    IMPORT  HandleInterrupt

DECL_ASMENTRY(IRQFromUSR)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    ldr     r4, irqStack
    add     r8, sp, #60         // offsetof(InterruptContext) = #60
    ldmia   r4, {r5 - r6}       // get saved pc, cpsr
    stmia   r8, {r5 - r6}       // save pc, cpsr
    stmdb   r8, {sp, lr}^       // save sp_usr, lr_usr
    mov     fp, #0

    mov     r0, sp
    bl      HandleInterrupt


    IMPORT  TryToSchedule

DECL_ASMENTRY(ReturnFromSystemCall)

DECL_ASMENTRY(ReturnFromUSRException)

DECL_ASMENTRY(ReturnFromUSRInterrupt)

    bl      TryToSchedule


DECL_ASMENTRY(RestoreUSRContext)

    mov     r0, #(PSR_I + PSR_F + PSR_MODE_SVC)
    msr     cpsr_c, r0          // disable irq

    ldr     r0, [sp, #64]       // offsetof(InterruptContext, cpsr) = #64
    msr     spsr, r0            // restore cpsr
    ldmia   sp, {r0 - lr}^      // restore r0 - lr
    mov     r0, r0
    ldr     lr, [sp, #60]       // get pc
                                // offsetof(InterruptContext, pc) = #60
    add     sp, sp, #68         // pop the InterruptContext
                                // sizeof(InterruptContext) = #68
    movs    pc, lr              // restore pc and cpsr


DECL_ASMENTRY(UNDFromSVC)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    add     r5, sp, #68         // sizeof(InterruptContext) = #68
    mov     r6, lr
    ldr     r7, undStack
    ldmia   r7, {r7 - r8}
    add     r4, sp, #52         // offsetof(InterruptContext, sp) = #52
    stmia   r4, {r5 - r8}       // save sp_svc, lr_svc, pc, cpsr

    mrs     r0, cpsr
    tst     r8, #(PSR_I + PSR_F)
    biceq   r0, r0, #(PSR_I + PSR_F)
    msr     cpsr_c, r0          // enable irq if it was

    mov     r0, #0              // ExceptionVector_UndefinedInstruction = #0
    mov     r1, #0
    mov     r2, sp
    adr     lr, ReturnFromSVCException
    b       HandleException


DECL_ASMENTRY(PABTFromSVC)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    add     r5, sp, #68         // sizeof(InterruptContext) = #680
    mov     r6, lr
    ldr     r7, abtStack
    ldmia   r7, {r7 - r8}
    add     r4, sp, #52         // offsetof(InterruptContext, sp) = #52
    stmia   r4, {r5 - r8}       // save sp_svc, lr_svc, pc, cpsr

    mrs     r0, cpsr
    tst     r8, #(PSR_I + PSR_F)
    biceq   r0, r0, #(PSR_I + PSR_F)
    msr     cpsr_c, r0          // enable irq if it was

    mov     r0, #1              // ExceptionVector_PrefetchAbort = #1
    mov     r1, #0
    mov     r2, sp
    adr     lr, ReturnFromSVCException
    b       HandleException


DECL_ASMENTRY(DABTFromSVC)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    add     r5, sp, #68         // sizeof(InterruptContext) = #68
    mov     r6, lr
    ldr     r7, abtStack
    ldmia   r7, {r7 - r8}
    add     r4, sp, #52         // offsetof(InterruptContext, sp) = #52
    stmia   r4, {r5 - r8}       // save sp_svc, lr_svc, pc, cpsr

    mrs     r0, cpsr
    tst     r8, #(PSR_I + PSR_F)
    biceq   r0, r0, #(PSR_I + PSR_F)
    msr     cpsr_c, r0          // enable irq if it was

    mov     r0, r7
	bl      DataAbort

    mov     r0, #2              // ExceptionVectorDataAbort = #2
    mov     r2, sp
    adr     lr, ReturnFromSVCException
    b       HandleException


DECL_ASMENTRY(IRQFromSVC)

    sub     sp, sp, #68         // sizeof(InterruptContext) = #68
    stmia   sp, {r0 - r12}      // save r0 - r12
    add     r5, sp, #68         // sizeof(InterruptContext) = #68
    mov     r6, lr
    ldr     r7, irqStack
    ldmia   r7, {r7 - r8}
    add     r4, sp, #52         // offsetof(InterruptContext, sp) = #52
    stmia   r4, {r5 - r8}       // save sp_svc, lr_svc, pc, cpsr

    mov     r0, sp
    bl      HandleInterrupt

DECL_ASMENTRY(ReturnFromSVCException)

DECL_ASMENTRY(ReturnFromSVCInterrupt)

    bl      TryToSchedule

DECL_ASMENTRY(RestoreSVCContext)

    mov     r0, #(PSR_I + PSR_F + PSR_MODE_SVC)
    msr     cpsr_c, r0          // disable irq

    ldr     r0, [sp, #64]       // offsetof(InterruptContext, cpsr) = #64
    msr     spsr, r0
    ldmia   sp, {r0 - pc}^

    IMPORT  StiAndEnablePreemption
    EXPORT  SystemThreadStartup
    EXPORT  ThreadStartup

DECL_ASMENTRY(SystemThreadStartup)

    bl      StiAndEnablePreemption

    mov     r0, r4
    mov     r1, r5
    mov     pc, r6


DECL_ASMENTRY(ThreadStartup)

    bl      StiAndEnablePreemption

    mov     r0, r5
    adr     lr, ReturnFromSystemCall
    mov     pc, r4


    END
